/**
 * @file Map.cpp
 * @brief 地图加载和管理实现
 */

#include "core/Map.h"
#include <fstream>
#include <iostream>
#include <cstring>
#include <opencv2/opencv.hpp>

namespace PathPlanning
{
// BMP文件头结构
#pragma pack(push, 1)
struct BMPFileHeader
{
    uint16_t fileType{0x4D42}; // "BM"
    uint32_t fileSize{0};
    uint16_t reserved1{0};
    uint16_t reserved2{0};
    uint32_t offsetData{0};
};

struct BMPInfoHeader
{
    uint32_t size{0};
    int32_t  width{0};
    int32_t  height{0};
    uint16_t planes{1};
    uint16_t bitCount{0};
    uint32_t compression{0};
    uint32_t sizeImage{0};
    int32_t  xPixelsPerMeter{0};
    int32_t  yPixelsPerMeter{0};
    uint32_t colorsUsed{0};
    uint32_t colorsImportant{0};
};
#pragma pack(pop)

Map::Map() {}

Map::~Map() {}

#if 0
bool Map::loadFromBMP(const std::string& filename)
{
    int width, height;
    if (!readBMPHeader(filename, width, height))
    {
        std::cerr << "not BMP header" << std::endl;
        return false;
    }

    std::ifstream file(filename, std::ios::binary);
    if (!file)
    {
        std::cerr << "Cannot open map file: " << filename << std::endl;
        return false;
    }

    BMPFileHeader fileHeader;
    BMPInfoHeader infoHeader;

    file.read(reinterpret_cast<char*>(&fileHeader), sizeof(fileHeader));
    file.read(reinterpret_cast<char*>(&infoHeader), sizeof(infoHeader));

    if (fileHeader.fileType != 0x4D42)
    {
        std::cerr << "Not a valid BMP file" << std::endl;
        return false;
    }

    if (infoHeader.bitCount != 24 && infoHeader.bitCount != 32)
    {
        std::cerr << "Only 24-bit and 32-bit BMP files are supported" << std::endl;
        return false;
    }

    width_       = infoHeader.width;
    height_      = std::abs(infoHeader.height);
    int channels = infoHeader.bitCount / 8;

    // 计算行填充
    int rowPadding = (4 - (width_ * channels) % 4) % 4;

    // 分配内存
    mapData_.resize(width_ * height_);

    // 移动到像素数据开始位置
    file.seekg(fileHeader.offsetData, std::ios::beg);

    // 读取像素数据
    for (int y = height_ - 1; y >= 0; --y)
    {
        for (int x = 0; x < width_; ++x)
        {
            uint8_t pixel[4];
            file.read(reinterpret_cast<char*>(pixel), channels);

            // 将RGB转换为灰度值
            uint8_t grayValue = static_cast<uint8_t>(0.299 * pixel[2] + 0.587 * pixel[1] + 0.114 * pixel[0]);

            // 反转值：0表示自由，255表示障碍
            mapData_[y * width_ + x] = 255 - grayValue;
        }
        file.seekg(rowPadding, std::ios::cur);
    }

    std::cout << "Map loaded successfully: " << filename << std::endl;
    return true;
}
#endif

bool Map::loadFromBMP(const std::string& filename)
{
    // 使用OpenCV加载图像
    cv::Mat image = cv::imread(filename, cv::IMREAD_ANYCOLOR);

    if (image.empty())
    {
        std::cerr << "Failed to load image: " << filename << std::endl;
        return false;
    }

    width_  = image.cols;
    height_ = image.rows;

    std::cout << "Image loaded successfully: " << filename << std::endl;
    std::cout << "Dimensions: " << width_ << " x " << height_ << std::endl;
    std::cout << "Channels: " << image.channels() << std::endl;

    // 转换为灰度图（如果需要）
    cv::Mat grayImage;
    if (image.channels() == 3)
    {
        cv::cvtColor(image, grayImage, cv::COLOR_BGR2GRAY);
    }
    else if (image.channels() == 4)
    {
        cv::cvtColor(image, grayImage, cv::COLOR_BGRA2GRAY);
    }
    else
    {
        grayImage = image;
    }

    // 分配内存存储地图数据
    mapData_.resize(width_ * height_);

    // 处理图像数据
    for (int y = 0; y < height_; ++y)
    {
        for (int x = 0; x < width_; ++x)
        {
            uint8_t grayValue = grayImage.at<uint8_t>(y, x);

            // 反转值：0表示自由，255表示障碍
            // 假设原图中白色(255)为自由空间，黑色(0)为障碍物
            mapData_[y * width_ + x] = 255 - grayValue;
        }
    }

    std::cout << "Map data processed successfully" << std::endl;
    std::cout << "Map size: " << width_ << " x " << height_ << std::endl;

    return true;
}
#if 0
bool Map::readBMPHeader(const std::string& filename, int& width, int& height)
{
    std::ifstream file(filename, std::ios::binary);
    if (!file)
    {
        return false;
    }

    BMPFileHeader fileHeader;
    BMPInfoHeader infoHeader;

    file.read(reinterpret_cast<char*>(&fileHeader), sizeof(fileHeader));
    file.read(reinterpret_cast<char*>(&infoHeader), sizeof(infoHeader));

    if (fileHeader.fileType != 0x4D42)
    {
        return false;
    }

    width  = infoHeader.width;
    height = std::abs(infoHeader.height);
    return true;
}
#endif

bool Map::readBMPHeader(const std::string& filename, int& width, int& height)
{
    cv::Mat image = cv::imread(filename, cv::IMREAD_UNCHANGED);
    if (image.empty())
    {
        return false;
    }
    width  = image.cols;
    height = image.rows;
    return true;
}

bool Map::isObstacle(int x, int y) const
{
    if (x < 0 || x >= width_ || y < 0 || y >= height_)
    {
        return true;
    }
    return mapData_[y * width_ + x] > 128;
}

bool Map::isObstacle(double worldX, double worldY) const
{
    GridCell cell = worldToGrid(worldX, worldY);
    return isObstacle(cell.x, cell.y);
}

bool Map::isObstacle(const Point& point) const
{
    return isObstacle(point.x, point.y);
}

GridCell Map::worldToGrid(double worldX, double worldY) const
{
    return GridCell(static_cast<int>(worldX / resolution_), static_cast<int>(worldY / resolution_));
}

GridCell Map::worldToGrid(const Point& point) const
{
    return worldToGrid(point.x, point.y);
}

Point Map::gridToWorld(int gridX, int gridY) const
{
    return Point(gridX * resolution_, gridY * resolution_);
}

Point Map::gridToWorld(const GridCell& cell) const
{
    return gridToWorld(cell.x, cell.y);
}

bool Map::isInBounds(int gridX, int gridY) const
{
    return gridX >= 0 && gridX < width_ && gridY >= 0 && gridY < height_;
}

bool Map::isInBounds(double worldX, double worldY) const
{
    GridCell cell = worldToGrid(worldX, worldY);
    return isInBounds(cell.x, cell.y);
}

bool Map::isInBounds(const Point& point) const
{
    return isInBounds(point.x, point.y);
}

void Map::printInfo() const
{
    std::cout << "=== Map Information ===" << std::endl;
    std::cout << "Dimensions: " << width_ << " x " << height_ << std::endl;
    std::cout << "Resolution: " << resolution_ << " m/pixel" << std::endl;

    int obstacleCount = 0;
    for (int y = 0; y < height_; ++y)
    {
        for (int x = 0; x < width_; ++x)
        {
            if (isObstacle(x, y))
                obstacleCount++;
        }
    }
    double obstacleRatio = static_cast<double>(obstacleCount) / (width_ * height_);
    std::cout << "Obstacle ratio: " << obstacleRatio * 100 << "%" << std::endl;
}

} // namespace PathPlanning